#include "can.h"
#include <time.h>
#include <chrono>
#include "DJI_Motor.h"
int main(){
    std::atomic<bool> running_;
    running_ = true;
    int target = 0;
    std::thread t1([&](){
        char c;
        while(1){
            std::cin >> target;
            if(target == 1)break;

        }
        running_ = false;
    });

    
    DJI_CAN_BUS can_bus0("can0");
    float Kp = 15;
    float Kd;
    can_bus0[1].set_Cascade_PID( 450,  0.1,  15,  0.03);
    // can_bus0[2].set_Vel_PID(15,0.03);
    // can_bus0[3].set_Angle_PID(1500,-6);
    while(running_){
        can_bus0[1].set_TargetVel(5000);
        can_bus0[1].set_TargetAngle(target);

        // can_bus0[2].set_TargetVel(500);

        // can_bus0[3].set_TargetAngle(target);
    }
    // DJI_CAN_BUS can_bus0("can0");
    // can_bus0[1].set_Angle_PID(1500,-6);
    // while(running_){
    //     can_bus0[1].set_TargetAngle(target);
    // }
    std::cout << "end" << std::endl;
    t1.join();
    std::cout << "t1 join" << std::endl;
    return 0;
}